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I.  INTRODUCTION 


A.  GENERAL 

The  weapons  being  developed  for  the  Strategic  Defense  Initiative  require  un¬ 
precedented  pointing  accuracies.  For  the  case  of  the  Space  Based  Laser  (SBL)  and 
Neutral  Particle  Beam,  the  pointing  accuracy  required  is  analogous  to  hitting  a 
beach  ball  on  the  Empire  State  Building  with  a  laser  on  Pike's  Peak  in  Colorado. 
The  problem  does  not  end  with  being  able  to  hit  the  beach  ball;  the  laser  has  to  illu¬ 
minate  the  target  for  a  specified  period  of  time.  The  United  States  Army  Strategic 
Defense  Command  has  a  precision  pointing  test  bed  located  near  Denver,  Colorado. 
This  facility  is  operated  by  the  Martin  Marietta  Corporation.  The  test  bed  facility, 
known  as  the  Rapid  Retargeting/Precision  Pointing  (R2P2)  facility  is  the  vehicle 
through  which  the  technologies  required  for  the  high  pointing  accuracies  and  rapid 
retargeting  are  being  developed  and  tested.  The  R2P2  facility  is  currently  config¬ 
ured  to  simulate  the  Space  Based  Laser,  the  inertial  reference  unit  and  the  various 
other  SBL  components. 

The  heart  of  the  R2P2  facility  and  the  SBL  is  the  fine  pointing  system.  The  fine 
pointing  system's  mission  is  to  keep  the  line  of  sight  of  the  weapon  system  pointed 
at  the  target.  Steering  mirrors  are  used  to  control  the  inertial  line  of  sight  angle. 
The  error  signal  received  by  the  steering  mirrors  can  be  treated  as  the  difference  of 
two  signals,  target  position  command  angle  (0  <  /  <  0.5  Hz)  minus  the  line  of  sight 
feedback  angle  (0  <  /  <  40 Hz).  The  angles  include  disturbances  such  as  command 
vehicle  motion  and  beam  expander  structural  vibration.  The  steering  mirrors  must 
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track  the  low  frequency  target  and  filter  the  high-  and  low-frequency  disturbances 
from  the  line  of  sight. 

Presently,  the  low  frequency  portion  of  the  steering  mirror  error  signal  is  pro¬ 
vided  by  the  Alignment  Inertial  Reference  (AIR)  platform.  Figure  1.1.  Due  to  the 
nature  of  the  application  and  the  type  of  sensor,  the  fine  tracker  operates  at  a  low 
sampling  rate  and  cannot  provide  high  frequency  information.  The  proposed  con¬ 
cept  is  to  use  the  AIR  platform  as  a  pseudo  target,  or  cooperative  target.  It  provides 
a  mirrored  surface  pointed  at  the  target  and  located  on  the  weapons  system.  An 
alignment  system  marker  beam  is  reflected  by  this  surface  and  a  sensor,  other  than 
the  fine  tracker,  is  used  to  obtain  line  of  sight  information.  This  alignment  sensor 
does  not  have  the  low  sample  rate  restriction  and  can  be  used  to  obtain  high  fre¬ 
quency  information.  The  command  signal  for  the  AIR  platform  is  formed  by  a  sum 
of  signals  from  the  fine  tracker,  the  alignment  sensor  and  the  AIR  platform  angle 
sensor. 

The  Strategic  Defense  Command  and  Martin  Marietta  desire  an  alternative 
approach  for  the  fine  pointing  system  on  R2P2.  The  improvement,  Figure  1.2, 
involves  eliminating  the  AIR  platform  from  the  loop.  Low  frequency  target  data  is 
obtained  from  the  fine  tracker,  which  samples  at  50  Hz.  A  second  signal  is  formed 
by  blending  the  output  from  two  sensors  that  measure  the  beam  expander  angle, 
a  strap-down  gyroscope  and  a  magneto-hydrodynamic  (MUD)  angular  vibration 
sensor.  The  strap-down  gyro  yields  low  frequency  information  while  the  MUD  is 
designed  to  give  high  frequency  observations.  The  difficulty  with  this  scheme  is 
the  blending  of  the  two  signals  to  produce  a  broad-band  measurement  of  the  beam 
expander  angle.  The  output  of  the  alignment  sensor  is  subtracted  from  the  output  of 
the  signal  mixer  to  yield  a  high  frequency  line  of  sight  angle  measurement.  A  second 
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signal  mixing  network  combines  this  signal  with  the  low  frequency  information  from 
the  fine  tracker. 

This  research  project  focused  on  the  mixing  of  the  measurements  from  the  two 
sensors,  the  gyroscope  and  the  MHD,  in  an  effort  to  fulfill  the  stated  requirements. 
Those  requirements,  put  forth  by  Martin  Marietta,  were: 

1.  Extremely  accurate  tracking  of  input  signal. 

2.  Extremely  fast  lock  on  time,  20  ms  or  better. 

3.  Flatness  in  magnitude  and  phase  for  the  combined  low  pass  and  high  pass 
sensors  as  shown  in  Figure  1.3. 

4.  Steep  cutoff  rates  for  the  outputs  of  the  individual  compensating  filters,  to 
minimize  noise  contributions  from  the  individual  sensors  in  their  non-valid 
regions  of  measurement. 

5.  A  selectable  blending  frequency,  selectable  at  any  point  between  u,’m  and  *,■ g  in 
order  to  blend  the  sensors  for  minimum  noise. 

6.  Minimal  sensitivity  of  the  compensating  network  to  parameter  variation  in  the 
sensors. 

7.  Minimum  number  of  poles  and  minimum  DC  gain  in  the  compensating  filters. 

To  meet  these  requirements,  a  Kalman  filter  was  designed  to  mix  the  outputs 
from  the  two  sensors.  The  Kalman  predicts  the  states  of  the  sensors,  discarding  the 
noise,  based  on  previous  measurements.  The  results  should  be  the  correct  frequency 
response  and  an  extremely  accurate  tracking  of  &bx •  The  results  of  the  Kalman 
filter  signal  blending  will  be  compared  with  the  signal  blending  filter  scheme  that 
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igure  1.1:  Currently  Implemented  Fine  Pointing  System 


Phase  (deg)  Magnitude  (db) 
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Figure  1.3:  Desired  Frequency  Response  for  System 

Martin  Marietta  has  devised,  utilizing  classic  filter  design.  Different  sensor  types 
will  be  compared  with  these  results  in  an  effort  to  further  improve  the  design. 
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II.  PROBLEM  STATEMENT 


A.  GENERAL 

The  filter  used  involves  two  sensors  with  different  bandwidths,  measuring  a 
common  input.  The  filter  then  blends  the  two  inputs  using  Kalman  techniques. 

The  problem  was  developed  using  state  space  methods.  Given  the  noise  clut¬ 
tered  input  angle,  9 ,  we  are  interested  in  the  noise-free  measurement  of  this  angle 
over  a  broad  band  of  frequencies.  The  state  variables,  (xl.  x2,  x3.  xl,  xo),  for  this 
plant  are  0,  9g,  9g<  and  0\f  as  defined  in  Table  2.1. 

TABLE  2.1:  STATE  VARIABLE  DEFINITIONS 


xl 

True  state  to  be  tracked 

9 

x2 

Gyroscope  angle 

@G 

1 3 

Gyroscope  angle  rate 

@G 

x4 

MHD  angle 

Om 

xo 

MHD  angle  rate 

Os, 

B.  SYSTEM  MODEL 

The  system  to  be  modeled  in  this  problem  is  that  of  an  inertial  reference  unit 
on  the  Space  Based  Laser.  In  the  development  of  this  work,  the  assumption  was 
made  that  all  noise  encountered  is  white  noise. 


Figure  2.1:  Simplified  Block  Diagram  for  Sensors 


The  transfer  functions  for  the  two  sensors  were  given  by  Martin  Marietta  [Ref.  1 
from  manufacturer  data  and  testing.  The  gyroscope's  transfer  function  is 

3947.8 


Hn(s)  = 


3 2  +  88.844s  +  3947.8 


(2.1) 


The  MUD  transfer  function  is 


Hl(s)  = 


t(3  +  2) 


s2  +  12.57s  +  157.91 


(2.2) 


Figure  2.2  shows  the  frequency  response  of  both  sensors.  It  can  be  seen  that  both 
sensors  are  second-order  systems. 

The  continuous  state  space  equations  for  the  modeled  system  are 


i  =  Ax  +  Dw 

8 


(2.3) 


y  =  Cx  +  v 


(2.4) 


where 


•  B  —  input  driving  function  matrix 

•  C  =  measurement  matrix 


•  xv  =  system  noise  matrix 

•  v  =  sensor  noise. 


and  the  state  vector  is 

'  e  ' 

x=  Og  (2.5) 

6\i 

Using  Equations  2.1  and  2.2  and  the  requirements  for  the  phase  and  magnitude  of 
the  output,  the  A  matrix  can  be  formed  as 

0  0  0  O' 

0  10  0 

-2C-6-  0  0  (2.6) 

0  0  0  1 

0  0  — u.’y/  —2  (,^m 

where 


•  ujc  —  gyro  cutoff  frequency 

•  u>m  =  MHD  cutoff  frequency 

•  £  =  damping  coefficient  for  each  sensor 
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For  the  model,  it  is  desired  that  the  fastest  reaction  time  possible  is  achieved. 
To  do  this,  the  system  is  critically  damped,  C  =  1 .  The  cutoff  frequencies  come  from 
sensor  specifications  and  testing.  Equations  2.1  and  2.2  reflect  the  cutoff  frequencies 
and  damping  coefficient  values  given.  The  classical  2nd  order  damped  system  has 
the  form 


#(.)  = 


u,2 


(2.7) 


s2  +  2  (u  +  u;2 

Discretizing  the  state  equations  yields  the  following  discrete  state  space  equa¬ 


tions, 


where 


xk+l  =  oxk  +  A  wk 


(2.S) 


•  xk  =  parameter  to  be  estimated  (State  Vector). 

•  (p  —  state  transition  matrix  which  describes  how  the  states  of  the  dynamic 
system  are  related. 

•  A  =  state  transition  matrix  for  input  driving  function. 

•  ivk  =  system  noise  matrix. 

From  Equation  2.8  and  the  above  assumptions,  the  <p  matrix  is 


1 

0 

0 

0 

0 

4.83  x  10"4 

0.999 

4.854  x  10"4 

0 

0 

1.913 

-1.913 

0.9886 

0 

0 

Xk 

1.966  x  10"5 

0 

0 

0.999 

4.96S  x  lO'4 

7.846  x  lO"2 

0 

0 

-7.846  x  10~2 

0.9875 

(2.9) 

The  system  noise  for  the  model  comes  from  the  input  that  the  sensors  are  measuring. 
This  input  will  have  noise  from  the  vehicle,  the  mirrors  and  the  beam  expander.  This 


C.  MEASUREMENT  MODEL 


For  a  linear  measurement  process,  the  measurements  are  linearly  related  to  the 
state  variables  and  can  be  modeled  using  the  discrete  linear  measurement  equation 
from  Equation  2.4, 

Zk  =  Hxk  +  vk  (2.10) 

where 


•  zk  =  set  of  measurements 

•  H  =  observation  matrix  that  gives  the  relationship  between  the  measurements 
and  the  state  vector 

•  xk  —  state  vector 

•  vk  —  measurement  noise  from  the  sensors 


With  the  appropriate  values  for  H,  Equation  2.10  becomes 

'  0  1  0  0  0  1 

Zk  ~  0  0  0  1  l  \Xk  +  Vk 


(2.11) 


In  this  blending  problem,  the  measurements  are  made  of  the  beam  expander 
by  the  sensors  that  make  up  the  inertial  reference  unit.  The  measurements  are  made 
noisy  by  the  noise  inherent  in  the  sensors.  The  sensors  have  been  rigorously  tested 
and  the  power  spectral  densities  have  been  computed  by  Martin  Marietta.  Figure 
2.3  shows  the  computed  noise  spectra  for  the  two  sensors. 

The  noise  from  the  sensors  is  a  function  of  many  variables  including  tem¬ 
perature  and  bandwidth  to  be  measured.  Although  this  is  generally  a  non-white, 
non-gaussian  noise  process,  it  can  be  adequately  described  as  a  white  noise  process 
over  an  extended  period  of  time. 
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Figure  2.3:  Noise  Spectral  Densities  for  Both  Sensors 
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The  state  and  measurement  equations  are  now  ready  to  be  implemented  in  the 
simulation.  What  follows  is  the  development  of  the  Kalman  fdter  equations  that  are 
the  heart  of  this  exercise. 
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III.  KALMAN  FILTER  THEORY 


A.  GENERAL 

Filtering  refers  to  the  process  of  estimating  the  state  vector  at  the  current  time, 
based  upon  all  past  measurements.  An  optimal  filter  concentrates  on  optimizing  a 
specific  performance  measure  used  to  approximate  the  quality  of  the  estimate.  The 
Kaiman  filter  is  the  optimal  filter  in  a  class  of  linear  filters  that  minimize  the  mean 
square  estimation  error  between  actual  and  desired  output.  In  other  words,  the 
Kalman  filter  attempts  to  minimize  the  elements  along  the  main  diagonal  of  the 
state  error  covariance  matrix.  The  Kalman  filter  has  been  used  extensively  in  the 
design  of  estimation  models  since  it  was  first  presented  by  Kalman  and  Bucy  [Ref. 
•5]  in  1960.  The  filter  itself  is  actually  a  recursive  algorithm  for  processing  discrete 
measurements  or  observations  in  an  optimal  manner.  [Ref.  6:p.  101]  ,1  priori 

knowledge  of  the  state  estimate  and  its  error  covariance,  and  the  current  observation 
is  required.  The  Kalman  filter  is  a  useful  algorithm  when  both  the  system  model 
and  the  measurement  model  are  linear  functions  of  the  state  variables  and  these 
models  can  be  described  by  the  equations 

Xfc-H  =  Oklk  +  -W  (3.1) 

zk  =  Hxk  +  I'k  (3.2) 

B.  SYSTEM  MODEL 

The  state  space  model  of  the  system  is  given  by  Equation  3.1  and  the  mea¬ 
surements  are  described  by  Equation  3.2.  This  is  a  standard  state  space  matrix 
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representation  for  a  system  of  linear  differential  equations.  In  E  quation  3.1.  i *  rep¬ 
resents  the  physical  state  and  represents  the  next  state  of  the  discrete  system. 

The  values  o  and  A  represent  the  discrete  time  state  matrices.  The  value  of 
Xjt  is  the  true  observed  parameters  of  the  state  and  iq  and  u-j.  are  observation  noise 
and  state  expectation  noise,  respectively. 

This  system  is  time  invariant  since  neither  o  nor  //  is  dependent  on  time. 
The  noise  processes  are  considered  to  be  stationary,  independent,  white  gaussian 
noise  with  zero  mean.  This  assumes  that  white  noise  is  an  idealization  of  nature's 
true  state;  however,  it  is  an  extremely  good  approximation  for  many  systems.  The 
statistical  properties  of  the  noise  are  given  below. 


£>*]  = 

0 

(3.3) 

\wjw1}  = 

Qtjk 

(3.4) 

E  M  = 

0 

(3.5) 

R6ik 

(3.6) 

E  juqe[] 

=  0 

(3T) 

The  matrices  Q  and  R  in  Equations  3.1  and  3.6  are  the  covariance  matrices 
for  the  noise  processes.  For  this  system,  the  noise  covariance  matrices  are  non-zero 
diagonal  matrices,  which  denote  the  power  present  in  the  noise.  This  model  will  be 
further  discussed  in  Chapter  4. 


C.  LINEAR  RECURSIVE  FORM 

Before  deriving  the  filter  equations,  the  form  of  the  filter  must  first  be  deter¬ 
mined.  The  form  assumed  for  most  Kalman  filters  is  shown  in  Equations  3.8  and 
3.9. 

•*>+11*  =  ^i-*-*!*  (3.8) 
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(3.9) 


•H+1|A:+1  —  ^2^k+l\k  +  ^'3~i  +  l 

The  current  estimate,  -r*+i|*+ii  is  a  linear  combination  of  the  previous  estimate. 
z*+i|fc,  and  the  current  observation,  This  form  is  chosen  for  its  simplicity,  but 

Reference  4  demonstrates  it  is  optima!  for  a  linear  system. 


D.  ERROR  COVARIANCE 

The  error  covariance  matrices  are  described  by  Equations  3.10  and  3.11. 


Pk+\\k 

Pk+\\k+l  z 


E  [ifc+n*ifc+1|*] 

(3.10) 

(3.11) 

These  matrices  give  a  feeling  for  the  expected  magnitude  of  the  estimation  error. 
Their  derivation  can  be  found  in  Reference  6:p.  10S.  The  Kalman  equations  begin 
to  take  shape  when  Equations  3.10  and  3.11  are  combined. 


Pk  +  l\k  —  $Pk\kOT  +  Q 


(3.12) 


Additionally,  writing  Equation  3.11  and  incorporating  the  equations  found  in  Ref¬ 
erence  2  in  the  development  of  the  covariance  matrix,  we  get 

ft+m+i  =  (/  -  OH)  Pk+,[k(I  -  GHf  +  GRGt  (3.13) 

where  G  is  the  Kalman  gain  matrix.  All  that  remains  is  to  find  the  value  of  this 
Kalman  Cain  matrix. 


E.  RESIDUAL  AND  VARIANCE 

The  definition  of  the  residual  will  be  helpful  in  simplifying  the  notation  re¬ 
quired  for  the  remainder  of  the  proof.  The  basis  for  the  residual  and  its  variance 
came  from  conversations  with  Steve  Spehn  [Ref.  7],  The  residual  is  given  by 

rk+ 1  —  zk+l  —  E[zk+\]  (3.14) 
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Since  the  estimate  is  unbiased,  we  see  that 


£[**+i]  =  E[Hxk+l]  +  E[vk+i]  (3.15) 

£[-Jt+i]  =  Hxk+nk  (3.16) 

By  substitution  and  algebra,  we  get  the  final  form  of  the  residual, 

rfc+i  =  Hik+\\k  +  t'fc+i  (3.17) 

(This  derivation  is  from  Reference  7.) 

The  covariance  of  the  residual  is  found  to  be 

var  [r*+1]  =  E  [rfc+1rj+1]  (3.18) 

var  [rfc+1]  =  HPk+x\kHT  +  R  ■  (3.19) 


Using  the  definition  of  the  residual,  the  observation  update  equation  can  be  written 
as 

•£fc+i|<fc+i  =  ifc+i|fc  +  Grk+l  (3.20) 

The  Kalman  Gain  equations  can  now  be  derived. 

F.  KALMAN  GAINS 

Solving  Equation  3.13  for  G  gives, 

Gk+ ,  =  Pk+l]kHT  (HPk+nkHT  +  /?)"'  (3.21) 

Recognizing  the  form  of  the  equation  in  parenthesis  to  be  that  of  Equation  3.19,  we 
simplify  to  the  final  form. 

Gk+ 1  =  P k+\\kHTvar  [r,t+i]-1  (3.22) 

Using  techniques  developed  in  Reference  3,  we  simplify  Equation  3.13  to 

P Wi|*+i  —  [I  ~  Gk+ \H]  Pk+i\k  (3.23) 

18 


G.  KALMAN  FILTER  EQUATIONS 

This  derivation  has  provided  a  set  of  recursive  equations,  which  give  a  time- 
varying  optimal  gain  matrix  and  a  detected  error  analysis  of  the  estimate.  The 


Kalman  filter  equations  are  given  below. 

ik+\\k  =  <t>xk\k  (3.24) 

Pk+i\k  =  0Pk\kOT  +  Q  ( 3 .25) 

Gk+l  =  Pk^\kHT  {HPk+x[kHT  +  R)~l  (3.261 

•ik+i\k+i  =  d'jt+iiA-  +  Gt+1  (cjt+i  —  i|itj  (3.27) 

P fc+pfc+i  =  (/  -  Gk+lH)  Pk+X\k  (3.28) 


These  equations  can  be  further  simplified  using  the  definitions  of  the  residual  and 
covariance  of  the  residual.  This  simplication  will  be  incorporated  in  the  simulations. 
Since  the  Kalman  equations  are  recursive,  they  are  readily  adaptable  to  computer 
simulation.  All  that  is  required  are  the  initial  conditions; 

x0|0'  Initial  estimate 
•Po|Oi  Error  covariance. 

This  a  priori  knowledge  is  essential  to  the  Kalman  process. 

The  Kalman  equations  are  now  ready  to  be  implemented  in  estimating  a  nor¬ 
mal  system.  The  next  step  is  to  make  the  Kalman  adaptable  thereby  increasing  its 
bandwidth.  This  will  be  accomplished  by  adding  maneuver  gating  to  the  Kalman 
filter. 
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IV.  MANEUVER  GATING 

A.  MAHALANOBIS  DISTANCE 

The  Mahalanobis  distance  (MD)  is  a  measure  of  the  derivation  of  the  obser¬ 
vation  from  the  estimate.  The  derivation  of  the  MD  is  found  in  Reference  8.  The 
idea  for  this  procedure  was  derived  from  Reference  7. 

The  Mahalanobis  distance  is  found  using  the  values  for  the  residual  and  co- 
variance  of  the  residual,  Equations  3.14  and  3.19, 

.l//)  =  r[+1i'ar[rkH]"1rHi  (4.1) 

The  resulting  scalar  is  compared  with  a  desired  threshold  in  the  program.  This 
threshold  was  picked  at  MD  =  4,  which  corresponds  to  the  statistical  2a  point  for 
the  noise  processes. 

B.  RESIDUAL  GATING 

Residual  gating  is  the  process  by  which  the  Kalman  adapts  itself  to  large  jumps 
in  the  observation.  The  system  being  tracked  in  this  simulation  can  be  expected 
to  have  large,  nearly  step-shaped,  changes  in  the  observations  being  tracked.  (The 
following  derivation  comes  from  Reference  7.) 

A  normal  Kalman  filter  would  observe  this  jump  and  initially  considers  it  as 
a  noise  perturbation.  The  Kalman  will  therefore  ignore  the  jump,  for  several  steps. 
If  the  large  value  persists,  the  filter  will  begin  to  react  with  speed  dependent  upon 
the  value  of  the  covariance  matrix,  P ,  at  the  time. 

This  reaction,  although  a  great  benefit  for  slow-moving  tracking  situations,  is 
extremely  restrictive  for  this  system.  The  requirement  for  lock-on  in  20  milliseconds 
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demands  a  more  proactive  Kalman  filter.  Residual  gating  provides  this  proactive 
behavior. 

Residual  gating  uses  the  Mahalanobis  Distance  derived  earlier  as  the  “gate" 
for  the  incrementation  of  the  covariance  matrix.  There  are  two  ways  for  a  Kalman 
filter  to  adapt,  either  by  increasing  the  gain  G't+i  or  the  covariance  matrix,  /\.  The 
covariance  matrix  was  selected  as  the  means  for  adaptation.  The  gate  is  set  up  using 
the  2(7  value  discussed  earlier.  A  value  of 

A/D  >4  (4.2) 

results  in  the  observation  falling  outside  the  gate  and  begins  the  adaptive  incre¬ 
menting  of  P, 

Pk\k  =  F  l’k\k  ■  (4.3) 

The  constant,  F,  was  used  to  adaptively  increase  the  last  value  of  the  covariance 
matrix,  Pk\k-  The  value  of  F  was  derived  experimentally  to  obtain  a  value  that 
results  in  optimal  filter  performance.  F  was  found  to  cause  little  variance  over  a 
wide  range  of  values. 

Through  analysis,  it  was  decided  to  use  a  gating  reset  of  P0jo.  This  results  in 
some  lag  time  in  the  filter,  which  is  made  up  for  by  its  faster  lock-on  time. 

The  next  step  in  the  design  is  to  simulate  the  inputs  and  scenarios  the  system 
will  see.  What  follows  are  the  various  simulations  used  to  tesi  the  filter's  ability  to 
meet  system  requirements. 
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V.  SIMULATIONS 


A.  SCENARIOS 

Several  scenarios  were  developed  for  this  simulation  to  test  its  applicability  to 
the  sensor  blending  problem.  In  all  scenarios,  observation  noise  was  present.  State 
excitation  noise  was  varied. 

1.  Scenario  One 

This  scenario  introduced  a  1  Hz  square  wave  with  various  noise  levels  into 
the  system.  Figure  5.1  shows  the  input  wave. 

2.  Scenario  Two 

This  scenario  introduced  a  10  Hz  square  wave  into  the  system  with  various 
noise  levels.  See  Figure  5.2. 

3.  Scenario  Three 

This  scenario  introduced  a  50  Hz  square  wave  into  the  system  with  various 
noise  levels.  See  Figure  5.3. 

4.  Scenario  Four 

The  input  for  this  scenario  is  a  100  Hz  square  wave.  This  input  is  the 
high  limit  provided  by  Martin  Marietta.  [Ref.  1]  See  Figure  5.1. 

B.  NOISE  INPUTS 

The  noise  inputs  for  the  model  were  developed  from  input  provided  by  Martin 
Marietta  [Ref.  1],  Figure  2.3  shows  the  noise  spectral  power  values  for  the  two 
sensors,  MHD  and  Gyro.  The  values  used  throughout  the  simulations  for  the  sensor 
noises  were  taken  as  the  median  from  the  graphs.  The  values  were  entered  as  t'c 
and  i'm ,  after  conversion. 


The  values  entered  for  state  excitation  noise,  u^,  were  derived  from  the  ex¬ 
pected  range  of  the  fine  tracking  system.  Varying  the  level  of  uy  enables  one  to  test 
the  robustness  of  the  model  and  filter,  lhe  mean  noise  level  was  selected  as  10~s 
rad. 

C.  RESIDUAL  GATING 

A  test  case  was  run  for  Scenario  One  input  without  residual  gating.  Figure  5.5 
shows  the  results  of  a  normal  Kalman  filter  without  residual  gating.  As  can  be  seen, 
the  performance  is  unacceptable  for  the  accuracy  requirements  stated.  It  will  serve 
as  a  good  reference  for  the  Kalman  filter  used  in  the  remainder  of  the  simulations. 


Figure  5.1:  Scenario  One 
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Figure  5.2:  Scenario  Two 

D.  REQUIREMENT  FOR  FREQUENCY  RESPONSE 

The  sponsor  of  this  thesis,  Martin  Marietta,  requested  a  frequency  response  of 
the  filtered  system  as  part  of  their  specifications  [Ref.  2].  The  Bode  plot  developed 
from  the  model  is  a  result  of  this  requirement. 

E.  BODE  FORMULATION 

A  bode  plot  is  a  plot  of  a  system  transfer  functions  response  over  a  range  of 
frequencies.  Martin  Marietta  desired  a  unity  gain  frequency  response  over  the  range 
of  interested  frequencies,  0.01-100  Hz.  [Ref.  1]  A  transfer  function  was  generated  us¬ 
ing  steps  put  forth  in  Reference  6  for  a  Wiener  steady  state  optimal  filter.  A  Wiener 
filter  is  an  optimal  filter,  identical  to  the  Kalman,  if  the  statistics  are  Gaussian.  The 
results  of  the  derivation  give  a  filter  transfer  function  of  the  form 

//(*)  =  [(zl  -<t>  +  GII)-'G]  (5.1) 
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Figure  5.3:  Scenario  Three 

The  transfer  function  was  derived  using  the  program  in  Appendix  D.  This 
transfer  function  was  combined  with  the  sensor  transfer  functions,  from  Figure  6.45, 
and  a  Bode  plot,  Figure  6.46,  was  obtained. 
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Figure  5.4:  Scenario  Four 
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Figure  5.5:  Scenario  One,  No  Residual  Gating 
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VI.  DISCUSSION  OF  RESULTS 


A.  GENERAL 

All  of  the  simulations  conducted  were  done  using  an  IBM-PC  and  the  software 
language  PC-MATLAB.  The  program  codes  are  contained  in  Appendices  A  and  B. 
The  results  achieved  could  not  be  shown  to  completely  satisfy  the  requirements  put 
forth  by  Martin  Marietta.  Specificially,  the  frequency  response  of  the  steady  state 
gain  Kalman  blended  system  did  not  meet  the  desired  specifications.  This  incon¬ 
sistency  was  resolved  by  the  adaptive  gating  incorporated  in  the  system  designed. 
This  will  be  discussed  in  detail  in  Section  VI-C. 

B.  KALMAN  PERFORMANCE 

The  performance  of  the  Kalman  filter  was  evaluated  through  several  steps  of 
increasing  noise  and  frequency  of  the  input.  The  filter  design  was  for  step  and  square 
wave  input,  as  per  Martin  Marietta’s  guidance  [Ref.  1].  The  Kalman  is  a  Type  0 
system,  by  design,  so  it  will  not  be  able  to  follow  a  ramp  or  sinusoid.  It  can  be 
modified  to  follow  those  two  inputs,  but  with  the  penalty  of  not  being  a  real-time 
system  any  longer.  The  system  this  filter  was  built  for.  the  R2P2,  is  extremely 
dependent  on  real-time  results.  Therefore,  the  Kalman  was  designed  to  be  as  fast 
as  possible. 

The  first  simulation  conducted  was  for  an  input  of  5  mrad  that  drops  to  0  mrad 
at  0.05  seconds  with  Q—0  and  R  ~  0.  The  R  matrix  could  not  be  made  to  equal  zero 
due  to  MATLAB  constraints.  This  simulation  will  act  as  a  baseline  for  which  the 
others  will  be  compared.  Figure  6.1  shows  the  input  and  filter  output.  Figure  6.2 
illustrates  the  error  between  the  actual  input  and  the  output  of  the  Kalman.  Figure 
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Figure  6.1:  Baseline  -  A'l  Estimation  for  Model  vs.  Input 


6.3  shows  the  value  of  the  mean  of  the  residual  over  the  period  of  the  simulation  and 
Figure  6.4  illustrates  the  ability  of  the  fdter  to  achieve  rapid  lock-on.  The  lock-on 
gate  used  in  these  simulations  is  i  20  /mad.  These  graphs  are  of  the  first  state  (x\) 
of  the  system,  which  is  the  state  we  are  concerned  with  following.  The  no-noise 
input  scenario  is  unrealistic,  but  is  effective  in  giving  a  baseline  for  the  rest  of  the 
analysis.  With  no  noise,  the  Kalman  is  able  to  lock-on  to  the  input  in  one  time 
step.  The  mean  of  the  residual  and  lock-on  time  are  two  wavs  of  checking  Kalman 
effectiveness.  They  will  be  used  in  analyzing  the  performance  of  the  Kalman  for  the 
scenarios. 
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Figure  6.4:  Baseline  -  System  Lock-On  Time 

C.  SCENARIO  RESULTS 
1.  Scenario  One 

Three  different  runs  were  made  for  Scenario  One,  in  which  the  noise 
inputs  were  varied.  The  input  signal  was  a  1  IIz  square  wave.  The  first  run  had  the 
state  noise  covariance,  Q ,  equal  to  0  and  the  measurement  noise  covariance  matrix 
equal  to  the  values  obtained  from  Figure  2.3.  Figures  6.5  to  6.8  show  the  simulation 
results.  Figures  6.9  to  6.12  show  the  results  of  the  next  run  in  which  noise  was 
introduced  into  the  Q  matrix  and  R  ~  0.  Figure  6.13  and  6.16  illustrate  the  results 
of  entering  representative  noise  into  both  the  Q  and  R  matrices. 

As  would  be  expected,  the  mean  of  the  residual  and  lock-on  times  were 
progressively  worse  for  each  case.  It  is  also  obvious  that  state  measurement  noise, 
R,  is  the  dominant  noise  input  in  the  filter.  The  results  of  the  final  run  of  this 
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Figure  6.5:  AT  Estimation  for  Model  vs.  Input 

scenario  are  well  within  the  desired  specifications.  The  Kalman  is  locking  on  with 
little  deviation  in  10-15  time  steps. 

2.  Scenario  Two 

This  scenario  takes  the  basic  system  and  applies  a  10  IIz  square  wave 
input  with  amplitude  of  ±  5  mrad.  The  values  for  Q  and  R  will  remain  constant 
for  the  remainder  of  the  scenarios.  Figures  6.17  to  6.20  show  the  results  of  entering 
the  10  Hz  wave  into  the  Kalman. 

For  this  input,  the  Kalman  performs  exceptionally  well  hock-on,  Figure 
6.20,  occurs  in  less  than  20  time  steps  and  the  mean  of  the  residual,  Figure  6.19, 
and  the  error,  Figure  6.18,  are  extremely  low. 

3.  Scenario  Three 

Scenario  Three  applied  a  50  Hz  square  wave  into  the  Kalman.  This 
frequency  of  input  appears  to  tax  the  Kalman’s  ability  to  follow  an  input.  Figure 
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Figure  6.7:  Mean  of  Error 
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Figure  6.10:  Plot  of  Error  Between  Estimate  and  Input  (XI) 


Figure  6.11:  Mean  of  Error 
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Figure  6.16:  System  Lock-On  Time 
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Figure  6.17:  AT  Estimation  for  Model  vs.  Input 
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Figure  6.20:  System  Lock-On  Time 

6.21  shows  the  Kalman  trying  to  track  the  input.  The  error  graph,  Figure  6.22, 
shows  the  output  gets  close  to  the  input  very  rapidly,  but  does  not  lock-on,  Figure 
6.24,  and  stay  there.  Partial  lock-on  is  achieved,  but  with  the  input  stepping  every 
20  time  steps,  the  Kalman  has  great  difficulty  getting  the  covariance  matrix  and 
gains  down.  There  appears  to  be  a  credible  performance  by  the  Kalman  at  this 
point,  but  it  is  pushing  its  abilities  with  the  present  specified  sample  rate  of  2  kHz. 

4.  Scenario  Four 

A  100  Hz,  5  mrad  square  wave  was  input  into  the  Kalman.  This  was  the 
specified  range  for  the  blending  filter  given  by  Reference  1.  Figures  6.25  and  6.28 
show  the  Kalman’s  inability  to  follow  an  input  of  this  high  of  a  frequency.  A  100 
Hz  wave  calls  for  a  step  up  or  down  every  10  time  steps.  In  other  words,  t!  e  input 
goes  through  two  complete  periods  in  the  required  lock-on  time  of  20  msec.  As  with 
Scenario  Three,  a  much  higher  sample  time  is  needed  for  the  Kalman  to  track  this 
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RADIANS  ^  RADIANS 


Figure  6.21:  A'l  Estimation  for  Model  vs.  Input 


Figure  6.22:  Plot  of  Error  Between  Estimate  and  Input  (A'l) 
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TIME  (sec) 

Figure  6.25:  AT  Estimation  for  Model  vs.  Input 

frequency  of  input.  The  zero  mean  error,  Figure  6.27.  results  from  how  the  mean  is 
computed  in  the  program.  The  program  allows  for  a  settling  time  of  13  time  steps 
after  gating.  The  100  Hz  wave  causes  the  filter  to  gate  every  10  time  steps.  The 
mean  cannot  be  computed  and  remains  zero. 

D.  NOISE  VARIATIONS 

In  order  to  verify  the  filter's  insensitivity  to  noise,  Scenario  Two  was  modified 
with  various  levels  of  state  noise  and  measurement  noise. 

The  first  simulation  decreased  the  values  in  the  Q  matrix  by  an  order  of  mag¬ 
nitude.  As  shown  in  Figures  6.29  through  6.32,  this  had  little  or  no  efFect  on  the 
outputs  when  compared  to  Figures  6.17  to  6.20. 

The  next  three  runs  involved  varying  the  R  matrix.  The  R  mat  rix  was  the  noise 
input  most  easily  influenced  in  the  system.  Variations  in  electrical  current  to  the 
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Figure  6.28:  System  Lock-On  Time 

sensors  or  misalignment  of  components  are  two  ways  that  could  increase  the  sensor 
noise.  To  increase  the  values  in  the  Q  matrix  would  require  a  failure  somewhere  in 
the  R2P‘2  damping  mechanisms  or,  in  real  life,  an  impact  on  the  structure  in  space. 

Therefore,  the  next  three  simulations  involved  increasing  the  magnitude  of  the 
noise  elements  \'c  and  V\f  of  ihe  R  matrix  by  factors  of  2,  5,  and  10.  The  resulting 
graphs  are  shown  in  Figures  6.33  through  6.44.  The  progression  of  the  simulations 
show  that  the  system  can  handle  up  to  an  order  of  magnitude  increase  in  noise  in 
both  sensors  and  still  function.  Figures  6.41  and  6.44  show  that  the  factor  of  10 
increase  does  push  the  system  to  the  limits  of  its  desired  capabilities.  Figures  6.33 
through  6.40  show  that  the  system  performs  quite  well  for  2  and  5  times  the  noise 
input. 
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Figure  6.29:  .VI  Estimation  for  Model  vs.  Input 
E.  FREQUENCY  RESPONSE 

The  final  portion  of  the  analysis  of  the  Kalman  filtered  system  was  the  fre¬ 
quency  response.  As  stated  earlier,  a  flat  response  over  the  interval  0.01-100  I Iz 
was  desired.  The  frequency  response  of  Martin  Marietta's  classical  blending  system, 
Figure  6.45,  is  shown  in  Figure  G.46.  The  frequency  response  for  the  steady  state 
gain  Kalman  filter  is  shown  in  Figure  6.47.  The  Kalman's  frequency  response  for 
the  steady-state  gains  does  not  meet  specifications.  Due  to  the  adaptive  gating  de¬ 
signed  into  the  Kalman  filter,  it  will  not  reach  the  steady-state  gain  values  utilized 
in  the  Wiener  development  under  normal  conditions.  With  any  kind  of  input,  the 
gains  will  be  adapting  continually.  The  steady-state  Kalman  approximation  meets 
the  required  error  requirements.  The  adapting  that  occurs  increases  the  bandwidth 
to  the  desired  range,  thereby  fulfilling  the  requirements. 
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Figure  6.38:  Plot  of  Error  Between  Estimate  and  Input  (AT) 
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Figure  6.43:  Mean  of  Error 
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Figure  6.47:  KALMAN  Blending  Scheme  (State  A  1) 
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VII.  CONCLUSION 


The  Bode  diagram  for  the  steady-state  Kalman  filter  clearly  shows  that  a 
steady-state  gain  filter  does  not  meet  the  bandwidth  requirements  for  the  blender. 
If  a  steady-state  Kalman  filter  had  been  used,  the  blending  scheme  proposed  would 
not  have  functioned  properly.  But  with  an  adaptive  gate  Kalman  filter,  the  signal 
blender  achieves  the  desired  bandwidth.  This  is  shown  in  the  various  simulations 
conducted.  The  purpose  of  an  adaptive  Kalman  is  to  adapt  the  bandwidth  of  the 
system  it  is  estimating.  The  Bode  shown  is  just  an  approximation  of  the  Kalman 
filter  developed.  It  is  a  snapshot  at  a  point  in  time  of  the  adaptive  filter.  Devel¬ 
oping  a  frequency  response  for  an  adaptive  Kalman  filter  is  a  possibility  for  further 
research. 

For  speed  and  accuracy,  the  Kalman  is  vastly  superior  to  the  classical  blending 
scheme.  Figure  7.1  shows  the  results  of  a  1  Hz  square  wave  input  into  the  Martin 
Marietta  system.  The  results  from  Scenario  One  are  orders  of  magnitude  better. 

The  adaptive  gating  approach  used  in  this  design  is  very  versatile  in  its  ap¬ 
plication.  Since  time  response  was  a  high  priority,  this  versatility  was  sacrificed  to 
a  degree.  By  adjusting  the  gate  and  factor,  F,  the  Kalman  filter  can  be  adapted 
to  follow  any  transient  input.  But,  the  faster  the  adaptation,  the  poorer  the  noise 
filtering  the  transient. 

Overall,  the  Kalman  filter  is  superior  to  the  classical  approach  to  blending 
two  signals.  For  speed  and  accuracy,  it  is  orders  of  magnitude  better.  With  a  few 
modifications,  it  can  be  made  to  follow  any  input. 
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APPENDIX  A:  MAIN  PROGRAM  AND 

INPUT  FILES 


All  of  the  simulations  for  this  project  were  run  on  1BM-PC  class  computers 
using  the  matrix  manipulation  language  MATLAB,  version  3. of.  This  appendix 
contains  the  source  code  for  all  of  the  functions  written  in  support  of  this  project. 

Only  minor  programming  experience  is  required  to  understand  these  files. 
\\  hile  MAI  LAB  is  similar  to  Fortran.  MATLAB  s  control  structures  are  much  less 
complex.  Comments  are  started  by  the  percent  sign  ( f/f )  and  continue  to  the  end  of 
the  line. 

To  aid  the  reader  in  scanning  and  retyping  these  functions,  each  file  is  started 
on  a  new  page.  Although  an  analysis  of  the  workings  uf  these  files  is  not  necessary 
to  understand  this  report,  the  curious  (or  skeptical)  reader  is  highly  encouraged  to 
examine  them  closely. 

I  he  author  neither  claims  nor  desires  to  hold  any  copyright  privileges  on  the 
source  code.  Written  requests  for  the  source  code  on  computer  disk  should  be  sent 
either  to  the  author  or  to  Professor  Harold  A.  Titus.  Address  information  can  be 
found  in  the  Initial  Distribution  List  at  the  end  of  this  report. 

All  of  the  files  listed  in  the  second  section  of  this  appendix  provide  general 
support  for  the  main  files  listed  in  the  first  section.  These  upport  files  are  not 
specific  to  the  simulations  run  for  this  report,  but  can  be  used  for  a  variety  of 


purposes. 


I 

% 


II  | 
I  #  THESIST5.M  30  JAN  90  # 
%  «  # 
%  I  MATLAB  Simulation  of  the  Beam  Expander  Inertial  Reference  # 
%  #  Unit  for  the  Rapid  Retargeting/Precision  Pointing  (R2/P2)  H 
I  #  System  I 
%  #  Before  running  this  simulation  the  length  for  the  # 
%  H  simulation  in  seconds  must  be  defined  as  the  variable  # 
I  ft  'kmax',  and  the  sampling  interval  is  defined  as  'dt'.  f 
I  #  The  program  uses  a  adaptive  gate  Kalman  filter  to  # 
%  #  blend  the  output  of  two  different  sensors.  The  sensors  # 
%  §  are  a  MagnetoHyrodynamic  rate  sensor  and  Singer  Rate  f 
1  It  Gyroscope.  I 
%  It  This  program  tracks  square  waves  of  different  I 
%  H  frequencies.  # 
I  I  H 


% 

I 

I 

% 

delete  bx.met  %  delete  previous  meta  file 

delete  output6.met 
clear ; 
clg  ; 

%  *******  INERTIAL  REFERENCE  UNIT  ***** 

I 

% 

I  The  IRU  is  made  up  of  two  sensors.  A  strap  down  gyro  and  a 
I  Magneto  HydroDynamic  rate  sensor. 

% 

|  ****  Input  Constants  **** 


b  =  1000*2*pi; 
wg  =  10.0*2*pi; 
wm  =  2.0*2*pi; 
zeta  =  1.0; 
dt  =  0.0005; 
kmax  =  0.20/dt+l; 
I  =  eye (5)  ; 
date=  26; 

IWg  =  1 e - 5 ; 

Wg  =  0; 

%Wm  =  1 e - 5 ; 

Wm  =  0; 


%end  break  freq 
Igyro  break  freq  down 
%MHD  break  freq  up 
%damping  ratio 
Isample  rate  for  system 


%  State  noise  for  gyro 


I  ******  Input  State  Matrices  For  Sensors 


*  *  *  *  * 


AS  =  zeros (5,5)  ; 

BS  =  [ 0 ; 0 ; Wg ; 0 ; Wm ] ; 
ZS  =  zeros (2,5)  ; 


%initialize  matrix  at  zero 
IB  matrix 

linitialize  observer  matrix 


I  ***  Enter  Values  Into  A  Matrix  *** 

% 

AS ( 1 ,  1 )  =  0; 

AS  (  2  ,  3  )  =  1  ; 
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AS (3,  1)  =  wg  *  2  ; 

AS  (3,  2)  =  -wg~2; 

AS  (3,3)  =  -2*zeta*wg; 

AS (4,5)  =  1 ; 

AS (5, 1)  =  vm"2 ; 

AS  (5,4)  =  -win* 2  ; 

AS ( 5 , 5 )  =  -2*zeta*vm; 

% 

%  ***  Enter  Values  Into  Observer  Matrix  4 

% 

ZS ( 1 , 2 )  =  1; 

ZS ( 2 , 4 )  =  1; 

ZS ( 2 , 5 )  =  .5; 

H  =  ZS  ; 

I 

%  **  Build  Observer  Matrix  ** 

I 

% 

%  ***  Discretize  State  Equations 

I 

[phi, del]  =  c2d (AS , BS , dt) ;  IDiscretize  states 

% 

%  ****  Construct  Kalman  Filter  Equations 

% 

%  **  Build  State  Noise  Error  Matrix 

wl  =  0; 
w2  =  le-2 ; 
w3  =  5e-5; 
w4  ■=  le-6; 
w5  =  le-5; 

Q  =  wl A  2  *eye ( 5 )  ; 
q  =  0 ;  %  1  e  -  5  ; 

% 

% 

%  **  Build  Measurement  Noise  Matrix  ** 

avg  =. 1 . 237e-6 ; 
avm  =  75.0e-6; 
vg  =  le-10*avg; 
vm  =  le-10*avm ; 
v  =  [vg  vm]'  ; 

R  =  zeros(2);  %R  matrix  values 

R  ( 1 » 1 )  =  vg-2; 

R  ( 2 , 2 )  =  vmA2  ; 

% 

%  **  Set  Initial  Error  Covariance  Matrix  ** 

% 

P  =  lel0*eye ( 5) ; 

P0  =  P; 

% 

% 

%  ****  Build  Kalman  Equations  **** 

\ 

y  =  zeros ( 2 , kmax) ; 
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m  =  zeros ( 5 , kmax)  ; 

tsql ; 

I 

for  i  =l:kmax, 

y(:,i)=  H*f ( : , i) +v.*rand(v) ; 
end 

I 

time  =  zeros ( 1 , kmax) ; 

time(l)  =  0; 

mr  =  zeros ( 2 , kmax) ; 

Xhat  =  zeros(5, kmax) ; 

Xhat ( : ,1)  =  [0  0  0  0  0] ' ; 
k_wait  =  0; 
meaner  =  [ 0 ; 0  ]  ; 


llnitial  estimate  of  states 
%  mean  of  the  residual 


for  k=2:kmax; 

Xhat ( :  ,  k)  =  phi*Xhat ( : , k-1) +del ; 
while  1 

resid  =  y  ( : ,  k) -H*Xhat ( : , k-1)  ; 

vresid  =  H*P*H'+R; 

md2  =  resid '/vresid*resid; 

if  md2  <  4  , 
break; 

else 

P  =  P0; 
kwait  =  0; 
mean_r  =  0*mean_r; 
mr ( : , k)  =  mean_r; 

end 


%X (k+l/k) 


Ivariance  of  the  residual 
%Mahalanobis  distance 
%gating  check 


%P(k/k) 


end 

k_wait  =  k_wait  +  1; 

G  =  P* (H) '/vtesid; 
p  =  ( I -G*H ) *P; 
p  =  phi*P* (phi) '+Q; 

Xhat ( : , k) =Xhat ( : ,k)+G*(y( :  , 
if  k_wait  >=  13 

kw  =  k_wait  -  13; 
mean_r  =  kw/ ( kw+ 1 )  *mean__r 
mr ( : , k)  =  mean_r; 

end 

time (k) =t ime ( k-1 ) +dt ; 
home ,  k 

end 


%Kalman  Gains  G(k+1) 
%P(k/k) 

IP(k+l/k) 

-H*Xhat  ( ;  , k) )  ;  %X(k+l/k+-l) 


+  1/ (kw+1) * resid; 


% 

%  ***  Calculate  the  Error  *** 

% 

error  =  m ( 1 , : ) -Xhat ( 1 ,  : )  ; 
errg  =  m (1 , : ) -Xhat (2 ,  : )  ; 
errm  =  m( 1 , : ) -Xhat (4 , : )  ; 

% 

%  **  Lock  on  Check  * 

1 

for  j=l:kmax, 

if  abs (error ( j )) <=  2e-5, 
lo(j)  =  0; 
else 

lo(j)  =  1.0; 
end ; 


end ; 

subplot (211) , plot (time ,  m  ( 1 ,  :) , time , Xhat ( 1 ,:),':' ) 

title ( 'XI  ESTIMATION  FOR  MODEL  VS.  INPUT') 

xlabel ( 'TIME  (sec) ' ) ,y label ( 'RADIANS' ) 

gtext ( ' STEP  INPUT  (-)  ') 

gtext( 'FILTER  OUTPUT  (:)  ') 

gtext ([' Noise  ' , num2str (q) , '  Rad']) 

subplot(212) , plot (time, error) .title ('PLOT  OF  ERROR  BETWEEN  ESTIMATE  AND  INPUT  (X 
xlabel ( 'TIME  (sec) ' ) , y label ( 'RADIANS' ) 
meta  output2 
pause 

%plot(time,m(l,  :)  , t ime , Xhat ( 2 , :)  ,  ':') 

Ititle ( ' XHAT  2  INPUT') 

lplot(time,errg) , title (  ' PLOT  OF  ERROR  BETWEEN  ESTIMATE  AND  INPUT  X2') 

%xlabel ( 'TIME  (sec) ' ) , y label ( 'RADIANS' ) 

%meta 

%pause 

%plot(time,m(l, :) , time , Xhat ( 4 , :) ,  '  :  ') 

%title( 'XHAT  4  INPUT' ) 

%plot(time, errm) ,title( 'PLOT  OF  ERROR  BETWEEN  ESTIMATE  AND  INPUT  X4 ' ) 
l xlabel ( 'TIME  (sec) ' ) ,  y label ( 'RADIANS' ) 

Imeta 

plot(time,mr(l, :)) ,title(' MEAN  OF  ERROR' ), xlabel ( 'TIME' ) 
axis ( [ 0  .20  -0.2  1.2]) ; 

plot(time, lo) , title( 'SYSTEM  LOCK  ON  TIME') 

xlabel ( 'TIME  (sec) ' ) 

meta 

pause 

clg ; 

axis ( ( 0  .20  -5e-5  5e-5])  ; 

plot(time, error) , title( 'PLOT  OF  ERROR  BETWEEN  ESTIMATE  AND  INPUT  (XI)') 

xlabel  TIME  (sec) '), ylabel (' RADIANS' ) 

meta 

axis; 
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I 

% 


%  *  # 

%  i  TSQ1.M  25  JAN  90  # 

%  j»  # 

I  «  MATLAB  Simulation  of  the  Beam  Expander  Inertial  Reference  I 

%  tf  Unit  for  the  Rapid  Retargeting/Precision  Pointing  (R2/P2)  # 

I  #  System  4 

%  0  Before  running  this  simulation  the  length  for  the  # 

%  #  simulation  in  seconds  must  be  defined  as  the  variable  jf 

%  4  'tm ax',  the  sampling  interval  is  defined  as  'dt'.  # 

%  #  This  program  generates  a  step  to  be  input  into  # 

1  I  THESIST5.M.  # 

I  #  # 


i 

% 

% 

dt=0 . 0005 ;  %sample  rate  for  system 

kmax=0 . 20/dt-t-l  ; 

% 

* 

%  ****  Build  Square  Wave  Input  **** 

y  =  zeros ( 2 , kmax) ; 
n  =  zeros ( 5 , kmax) ; 


for  i=  l:kmax; 
if  i<=100 , 


state  =  [ . 005 

.005  0 

else 

state  =[000 

0  0  ]  '  ; 

end 

f ( : , i ) =  state  ; 

m ( : , i )  =  state ; 

end 
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% 

% 

%  ################################################################ 


I  <  I 

I  #  TSQ10.M  25  JAN  90  I 
%  d  I 
I  I  MATLA8  Simulation  of  the  Beam  Expander  Inertial  Reference  I 
%  d  Unit  for  the  Rapid  Retargeting/Precision  Pointing  (R2/P2)  d 
%  d  System  d 
%  d  Before  running  this  simulation  the  length  for  the  d 
%  d  simulation  in  seconds  must  be  defined  as  the  variable  d 
%  d  'tmax',  the  sampling  interval  is  defined  as  'dt'.  d 
*.  d  This  program  generates  a  10  Hz  square  wave  to  be  d 
1  d  input  into  TliESIST.M.  d 
%  It  d 


%  dddddddddddMdddddddddddddddddddddddddddddddMdddddddddddddddddd 
% 

% 

% 

% 

dt=0.0005;  Isample  rate  for  system 

kmax=0 . 50/dt+l ; 

% 

I 

%  ****  Build  Square  Wave  Input  **** 

y  =  zeros (2 , kmax) ; 
m  =  zeros ( 5 , kmax) ; 

for  i=  l:kmax; 
if  i<=100, 

state  =  [.005  .005  0  .005  0 ]  '  ; 
elseif  i <=2 00 

state  =  - 1  *  [  .005  .005  0  .005  0]'; 
elseif  i<  =  3 0 0 

state  =  [.005  .005  0  .005  C]'; 
elseif  i<  =  4 0 0 

state  =  -1  * [ . 005  .005  0  .005  0]'; 
elseif  i<=500 

state=  [.005  .005  0  .005  0]'; 
elseif  i<=600 

state  =  -1*[.005  .005  0  .005  0 ]  '  ; 
elseif  i <=7 00 

state=  [.005  .005  0  .005  0]'; 
elseif  i<=800 

state  =  -1* [ .005  .005  0  .005  0]'; 
elseif  i<=900 

state=  [.005  .005  0  .005  0]'; 
else 

state  =  - 1  * [ .005  .005  0  .005  0]'; 

end 

f ( : , i ) =  state ; 
m ( : , i)  =  state ; 
end 
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I 

I 


%  f  I 

%  f  TSQ50.M  25  JAN  90  i 
%  «  # 
%  f  MATLAB  Simulation  of  the  Beam  Expander  Inertial  Reference  I 
I  §  Unit  for  the  Rapid  Retargeting/Precision  Pointing  (R2/P2)  # 
%  §  System  # 
%  f  Before  running  this  simulation  the  length  for  the  # 
%  H  simulation  in  seconds  must  be  defined  as  the  variable  # 
\  #  'tmax',  the  sampling  interval  is  defined  as  'dt'.  f 
I  i  This  program  generates  a  50  Hz  square  wave  to  be  ft 
%  |  input  into  THES1ST.M.  # 
%  f  * 


i 

% 

% 

% 

dt=0.0005;  %sample  rate  for  system 

kmax=0 . 10/dt+l ; 

I 

% 

%  ****  Build  Square  Wave  Input  **** 

y  =  zeros ( 2 , kmax) ; 
m  =  zeros ( 5 , kmax) ; 

for  i=  l:kmax; 
if  i<=20, 

state  =  [.005  .005  0  .005  0]'; 
elseif  i<=40 

state  =  - 1  * [ . 00 5  .005  0  .005  0]'; 
elseif  i<=60 

state  =  [.005  .005  0  .005  0 ] '  ; 
elseif  i<=80 

state  =  - 1  * [ . 005  .005  0  .005  0]'; 
elseif  i<=100 

state=  [.005  .005  0  .005  0]'; 
elseif  i<=120 

state  =  -1  * [ .005  .005  0  .005  0]'; 
elseif  i<=140 

state=  (.005  .005  0  .005  0]'; 
elseif  i<=160 

state  =  -1* [ . 005  .005  0  .005  0]'; 
elseif  i<=180 

state=  (.005  .005  0  .005  0]'; 
else 

state  =  -1* ( . 005  .005  0  .005  0]'; 

end 

f ( : , i ) =  state ; 
m  ( :  ,  i )  =  state ; 
end 
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% 

% 

\ 

% 

V 

% 

% 

I 

* 

\ 

\ 

\ 

I 

% 

% 

% 

1 

% 


f!flH***#*«*«H*«H#**»#**H**HH*#«****HHH*#*«IH*#*##*H«« 


i  I 

I  TSQ100.M  25  JAN  90  * 

I  « 

I  MATLAB  Simulation  of  the  Beam  Expander  Inertial  Reference  t 

I  Unit  for  the  Rapid  Retargeting/Precision  Pointing  (R2/P2)  I 

#  System  f 

I  Before  running  this  simulation  the  length  for  the  # 

I  simulation  in  seconds  must  be  defined  as  the  variable  f 

I  'tmax',  the  sampling  interval  is  defined  as  'dt'.  # 

I  This  program  generates  a  100  Hz  square  wave  to  be  # 

*  input  into  THESIST.M.  i) 

a  i 


% 

dt=0.0005;  %sample  rate  for  system 

kmax=0 . 0  5/dt+ 1 ; 

1 

I 

t  ****  Build  Square  Wave  Input  **** 

y  =  zeros ( 2 , kmax) ; 
m  =  zeros ( 5 , kmax) ; 

for  i=  l:kmax; 
if  i<=10( 

state  =  [.005  .005  0  .005  0]'; 
elseif  i<=20 

state  =  -1* [ . 005  .005  0  .005  0 ]  '  ; 
elseif  i<=30 

state  =  [.005  .005  0  .005  0]'; 
elseif  i<=40 

state  =  -I* [ .005  .005  0  .005  0]'; 
elseif  i<=50 

state=  [.005  .005  0  .005  0]'; 
elseif  i<=60 

state  =  - 1 * [ .005  .005  0  .005  0]'; 
elseif  i<=70 

state=  [.005  .005  0  .005  0]'; 
elseif  i<=80 

state  =  - 1 * [ .005  .005  0  .005  0]'; 
elseif  i<=90 

state=  [.005  .005  0  .005  0)'; 
else 

state  =  -1* [ . 005  .005  0  .005  0]'; 

end 

f ( : , i) =  state; 
rn( : ,  i)  =  state; 
end 
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APPENDIX  B:  BODE  PROGRAMS 


This  appendix  contains  the  programs  used  to  compute  the  Bode  diagrams 
contained  in  the  main  body  of  the  thesis. 
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THESBDE.M 


1  NOV  8  9 


% 

% 

% 

%  MATLAB  simulation  of  Kalman  Filter  signal  blending 
%  scheme.  Developed  by  the  Terry  J  Bauer,  CPT  USA,  for 
I  the  R2P2  fine  tracking  system. 

% 

% 

'.delete  thbode.met 
Idelete  tbode.met 

% 

%  Enter  the  Transfer  Functions  For  The  Network 

% 

numm=[78.95  157.91];  %MHD  sensor 

denm= (  1  12.57  157.91]  ; 

numg=[0  3947.8];  IGyro  sensor 

deng= [  1  62.83  3947.8  ]  ; 

w=  logspace ( -2 , 3 )  ;  %frequency  range 

% 

I 

I 

%  ***  Enter  Kalman  Values 

% 

I  =  eye ( 5)  ; 
wg  =  1 0  *  2  *  p  i  ; 
wm  =  2*2*pi; 
dt  =  .0005; 

A  =  [0  0  0  0  0; 

00100; 

wg‘2  -wg ' 2  -2*wg  0  0; 

00001; 

wnr2  0  0  -wm'2  -2*wm]; 

B  =  [0  0  le-5  0  le-5 ]  '  ; 

Q  =  (le-5)  ■'2*1; 

R  =  [ (1.237e-6) "2  0 ; 0  (75e-6)"2]; 

H  =  (0  1  0  0  0 ; 0  0  0  1  .5]; 

[phi, del]  =  c 2 d ( A , B , d t )  ; 

L  =  [0  0  1  0  1  ]  '  ; 

D  =  (0  0 ; 0  0 ; 0  0 ; 0  0 ; 0  0 ]  ; 

1 

\  +**  Bode  for  Kalman 

I 
* 

G  =  d lqe ( ph i , 1 , H , Q , R)  ; 

[ numkl , denkl  ]  =  ss2tf(phi+G*H,G,I,D,l); 

( numk2 , denk? ]  =  ss2tf(phi+G*K,G,I,D,2) ; 

% 

\  **  Combine  Transfer  Functions  ** 

I 

nutl  =  conv (numg , numkl ( 1 ,:) )  +  conv(numm,numk2 (1, ; ) ) 
nut2  =  conv ( numg , numkl ( 2 ,:) )  +  conv (numm, numk2 ( 2 , : ) ) 
nut3  =  conv(numg, numkl (3, : ) )  +  conv (numm, numk2 ( 3 , : ) ) 
nut4  =  conv ( numg , numkl ( 4 ,;) )  +  conv(numm, numk2 (4 , : ) ) 
nut5  =  conv ( numg , numkl ( 5 ,:) )  +  conv ( numm ,  numk2 ( 5  ,  :  )  ) 
det  =  conv (deng , denkl )  +  conv (denm, denk2 ) ; 

%nutl  =  numklfl,;); 


IGyro  break  freq 
%MHD  break  freq 
%Sample  ratye 


09 


Inut2  =  numkl  ( 2 ,  : )  ; 
lnut3  =  numkl ( 3 , : )  ; 

Inut4  =  numkl ( 4 , : )  ; 

Inut5  =  numkl ( 5 ,  :  )  ; 

% 

%  Calculate  Bode  Frequency  Response 

I 

(magi ,  phasel  ]  =  bode  ( nut  1 ,  det ,  w)  ,* 

( mag2 , phase2 ] =  bode (nut2 , det , w) ; 

( mag3 , phasel j =  bode ( nut3 , det , w) ; 

[mag4 , phase4 ] =  bode ( nut4 , det , w) ; 

[ mag5 , phase5 ] =  bode ( nut5 , det , w) ; 
clg 

semilogx (w, 20*logl0 (magi) ), title ( 'KALMAN  Blending  Scheme  (State  XI)') 
xlabel('Frequency') , y label ( ' Magnitude  (db) ' ) ,grid 
meta  thbode 
pause 

Isemilogx ( w, phasel) , title (' KALMAN  Blending  Scheme  (XI)') 

lxlabel('F  requency ' ) , y labe 1 ( ' Phase  (deg)'), grid 

%pause 

Imeta 

semilogx (w, 20*logl0 (mag2) ), title (' KALMAN  Blending  Scheme  (X2)') 

xlabel ( 'Frequency' ) , y label ( 'Magnitude  (db) ' ) , grid 

meta 

pause 

Isemilogx (w, phase2 ), title (' KALMAN  Blending  Scheme(X2)') 

I xlabel ( 'Frequency' ) , y label ( 'Phase  (deg) ' ) , grid 

Imeta 

Ipause 

semilogx (w, 20*1 og 10 (mag 3 ) ) , title ( 'KALMAN  Blending  Scheme (X3) ' ) 

xlabel ( ' Frequency' ) , y label ( 'Magnitude  (db) ' ) , grid 

meta 

pause 

%semilogx (w, phase3) , title (' KALMAN  Blending  Scheme(X3)') 

% xlabel ( ' Frequency ' ) , y label ( ' Phase  (deg) ' ) , grid 

Imeta 

Ipause 

semilogx (w,  20*logl0 (mag4 )), title (' KALMAN  Blending  Scheme(X4)') 
xlabel ( ' Frequency' ) , y label ( 'Magnitude  (db) ' ) , grid 
meta 
pause  . 

isemilogx (w, phase4) , title (' KALMAN  Blending  Scheme  (X4)') 

% xlabel ( ' Frequency' ) , y label ( ' Phase  (deg) ' ) , grid 

Imeta 

Ipause 

semilogx(w,  20*logl0 (mag5) ), title (' KALMAN  Blending  Scheme(X5)') 

xlabel ( 'Frequency' ) , y label ( 'Magnitude  (db) ' ) , gr id 

meta 

Ipause 

Isemilogx (w,  phase5) , title (' KALMAN  Blending  Scheme  (X5)') 

I xlabel ( ' Frequency ' ) , y label ( ' Phase  ( deg) ' ) , grid 
Imeta 
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THESBDE.M 


1  NOV  8  9 


% 

% 

% 

1 

\ 

\ 

% 

% 

% 

% 


MATLAB  simulation  of  Martin  Marietta's  signal  blending 
scheme.  Developed  by  the  R2/P2  Control  Group  in  Denver, 
Colorado . 


Enter  the  Transfer  Functions  For  The  Network  And  Blender 


rumm= [1  0  0 ] ; 

d"*nm=  [  1  12.57  157.91]; 

.  ...mmf=( 1  0  0  ]  ; 

denmf = (  1  62.8  3944  ]  ; 

numg= [  3947.8  ]  ; 

deng=[ 1  62.83  3947.8]; 

numg  f = [ 157.8]  ; 

dengf = [  1  12 . 56  157 . 8  ]  ; 

numbl=[ 398  30000  3.77e5  3.94e6 

denbl= [ 1  1062.8  66744  3.94e6]; 

w=  log space ( -2 , 3 ) ; 

% 


%MHD  sensor 
%MHD  filter 
%Gyro  sensor 
%Gyro  filter 

%Blender 
Ifrequency  range 


I  Combine  Transfer  Functions  Along  Branches 
% 


nunmt=conv(numm,nummf) 
denmt=conv (denm, denmf ) ; 
cl=conv(numg,numgf) 
c2=conv (deng, dengf ) 
numgt=conv(cl,numbl) 
dengt=conv (c2 , denbl ) ; 


ldifl=  length (numgt)  -  length ( nummt) ; 
if  ldifl  >=0 

nummt  =  [ zeros ( 1 , ldifl)  nummt]; 
else 

numgt  =[  zeros ( 1 ,  abs ( ldifl )  )  numgt]; 

end 

ldif2=  length (dengt)  -  length (denmt) ; 
if  ldif2  >=0 

denmt  =  [ zeros ( 1 , ldif 2 )  denmt]; 
els^ 

dengt  =(  zeros ( 1 , abs ( ldi f 2 ) )  dengt]; 

end 


numeq=nummt  +numgt;  Isum  of  the  sensors 

deneq=denmt+dengt; 

t 

I  Calculate  Bode  Frequency  Response 

% 

(mag,phase]=  bode (numeq,deneq, w)  ; 
clg 

semilogx ( w, 2 0*logl0 (mag) ) , t itle ( ' MMAG  Blending  Scheme') 
x label ( 'Frequency ' ) , ylabel ( ' Magnitude  (db) ' ) , grid 
meta  tbode 
pause 

sem i logx ( w, phase) , tit le (' MMAG  Blending  Scheme') 
x label ( ' Frequency ' ) , ylabel ( ' Phase  (deg) ' ) , grid 
meta 
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% 

% 

I  THESC2D. m  8  DEC  1989 

% 

% 

%  ,  This  program  converts  the  transfer  functions  to  state  space 

%  reprensentations .  It  will  then  discretize  the  state  space 

%  representation. 

% 

% 

%  Enter  transfer  functions 

I 

numl= [ 0  0  3947.8] ; 
denl=[  1  88.844  3947 . 8  ]  ; 
num2= [ 1  0  0  ]  ; 
den2= [ 1  12.97  157.91]  ; 

[al,bl,cl,dl]=tf2ss( nural , deni)  ; 

[a2,b2,c2,d2]=tf2ss(num2(den2)  ; 

I 

%  Convert  to  discrete  time 

1 

dt=0 . 0005 ; 

[phil,dell]=c2d (al,bl,dt) 

[phi2,del2]=c2d(a2,b2,dt) 

% 

I  Find  discrete  5x5  for  whole  system 

% 

a= [ 0  1000 

-3947.8  -88.844  000 
0  0  0  1  0 

0  0  -157 .91  -12 . 57  0 
0  0  0  0  -1000]  ; 

I 

b= [ 0  -3947.8  0  -157.91  1000]'; 

I 

[phi , del ] =c2d (a , b , dt)  ; 


phit=phil  +  phi2 ; 
delt=dell+del2  ; 
at=al  +  a2 ; 
bt=bl+b2 ; 
ct=cl  +  c2 ; 
dt=dl+d2  ; 
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